/*
 * task_transfer.h
 *
 *  Created on: 2025 Jul 6
 *      Author: SFLYs
 */

#ifndef XTSPINELIB_TASKS_TASK_TRANSFER_H_
#define XTSPINELIB_TASKS_TASK_TRANSFER_H_

#include "bsp_canfd.h"
#include "bsp_led.h"
#include "bsp_spi.h"
//#include "bsp_spi_dma.h"
#include "hal_canfd_communicator.h"
#include "hal_spi_communicator.h"
#include "mod_math.h"
#include "string.h"

// length of receive/transmit buffers
#define RX_LEN 130
#define TX_LEN 130

// length of outgoing/incoming messages
#define DATA_LEN 58 //29*2

#define CMD_LEN  130 //65*2

// Master CAN ID ///
#define CAN_ID 0x0


/// Value Limits ///
 #define P_MIN -12.5f
 #define P_MAX 12.5f
 #define V_MIN -65.0f
 #define V_MAX 65.0f
 #define KP_MIN 0.0f
 #define KP_MAX 500.0f
 #define KD_MIN 0.0f
 #define KD_MAX 5.0f
 #define T_MIN -18.0f
 #define T_MAX 18.0f

 /// Joint Soft Stops ///
 #define A_LIM_P 1.5f
 #define A_LIM_N -1.5f
 #define H_LIM_P 5.0f
 #define H_LIM_N -5.0f
 #define K_LIM_P 0.2f
 #define K_LIM_N 7.7f
 #define KP_SOFTSTOP 100.0f
 #define KD_SOFTSTOP 0.4f;

#define ENABLE_CMD 0xFFFF
#define DISABLE_CMD 0x1F1F


//28+1 32bit data
//58   16bit data
typedef struct __attribute__((packed)){
    float q_hip[4];
    float q_thigh[4];
    float q_calf[4];
    float v_hip[4];
    float v_thigh[4];
    float v_calf[4];
    int32_t flags[4];
    int32_t checksum;
}spi_data_t; //SPINE---->PC


//64+1  32bit data
//130   16bit data
typedef struct __attribute__((packed))
{
    float q_des_hip[4];
    float q_des_thigh[4];
    float q_des_calf[4];
    float v_des_hip[4];
    float v_des_thigh[4];
    float v_des_calf[4];
    float kp_hip[4];
    float kp_thigh[4];
    float kp_calf[4];
    float kd_hip[4];
    float kd_thigh[4];
    float kd_calf[4];
    float tau_hip_ff[4];
    float tau_thigh_ff[4];
    float tau_calf_ff[4];
    int32_t flags[4];
    int32_t checksum;
}spi_command_t;//PC---->SPINE


typedef struct{
    float p_des, v_des, kp, kd, t_ff;
}joint_control_t;

typedef struct{
    float p, v, t;
}joint_state_t;

typedef struct{
    joint_state_t hip;
    joint_state_t thigh;
    joint_state_t calf;
}leg_state_t;


typedef struct{
    leg_state_t L_Leg_state;
    leg_state_t R_Leg_state;
}Leg_State_t;



typedef struct{
    joint_control_t hip;
    joint_control_t thigh;
    joint_control_t calf;
}leg_control_t;

void Task_Board_Init(void);
void Task_Pack_Cmd(xt_hal_canmessage_t * msg, joint_control_t joint);
void Task_Unpack_Reply(xt_hal_canmessage_t msg, Leg_State_t * leg);
void Task_EnterMotorMode(xt_hal_canmessage_t * msg);
void Task_ExitMotorMode(xt_hal_canmessage_t * msg);
uint32_t xor_checksum(uint32_t* data, size_t len);
void Task_control(void);
void Task_PackAll(void);
void Task_WriteAll(void);
int Task_softstop_joint(joint_state_t state, joint_control_t * control, float limit_p, float limit_n);
void Task_LowPriorityEvent(void);
#endif /* XTSPINELIB_TASKS_TASK_TRANSFER_H_ */
